#include <fingerprint.h> 

// 创建软串口对象
SoftwareSerial fingerprintSerial(FINGERPRINT_RX, FINGERPRINT_TX);
// 定义缓冲区ID
uint8_t BUFFER_ID = 0;



// 全局变量
bool isInit = false;
void printHex(uint8_t* data, uint8_t len) {
  for(uint8_t i=0; i<len; i++){
    if(data[i] < 0x10) Serial.print('0');
    Serial.print(data[i], HEX);
    Serial.print(' ');
  }
  Serial.println();
}

void initfingerprint() {
  // 初始化串口
 // Serial.begin(9600);
  fingerprintSerial.begin(57600);
  while (fingerprintSerial.available()) fingerprintSerial.read();
//while (Serial.available()) Serial.read();

 // delay(1000); // 等待串口稳定
}


void command_use(void)
{
  Serial.println("------------------------------------------");
  Serial.println("command input:");
  Serial.println("\"1\" means \"register fingerprint.\"");
  Serial.println("\"2\" means \"search fingerprint.\"");
  Serial.println("\"3\" means \"clear fingerprint.\"");
  Serial.println("------------------------------------------");
}
bool run_command(void)
{
  if (Serial.available() > 0) {
    String command = Serial.readStringUntil('\n');
    command.trim();

    if (command.length() == 0) return false; // 忽略空命令

    if (command == "1") {
      Serial.println("[INFO] registering...");
      if(register_FP()) {
        Serial.println("FP registration process complete!");
      } else {
        Serial.println("FP registration process fail! Please 'register' again!!!");
      }
    } 
    else if (command == "2") {
      Serial.println("[INFO] searching...");
      if(search_FP()) {
        Serial.println("FP match succ!");
      } else {
        Serial.println("FP match fail! Please 'search' again!!!");
      }
    } 
    else if (command == "3") {
      Serial.println("[INFO] clearing...");
      if(clear_FP_all_lib()) {
        Serial.println("FP clear FP all lib succ!");
      } else {
        Serial.println("FP clear FP all lib fail! Please 'clear' again!!!");
      }
    } 
    else {
      Serial.print("[ERROR] unknown cmd: ");
      Serial.println(command);
    }

    // 清空残留串口数据，防止重复执行
    while (Serial.available() > 0) Serial.read();
    return true;
  }

  return false;
}


//读模组基本参数
int read_FP_info(void)
{
uint8_t response[32];
uint8_t index = 0;
uint32_t startTime = millis();

Serial.println("ZW101 FPM info:");
send_cmd(CMD_READ_SYSPARA);

// 等待包头 EF 01
while (millis() - startTime < 2000) {
  if (fingerprintSerial.available()) {
    uint8_t b = fingerprintSerial.read();
    // 过滤杂散数据，等待包头开始字节 0xEF
    if (index == 0 && b != 0xEF) continue;
    // 如果是包头第二字节不是 0x01，则重置
    if (index == 1 && b != 0x01) {
      index = 0;
      continue;
    }
    response[index++] = b;
    // 包长度计算，等接收完数据包后跳出循环
    if (index >= 9) { 
      // 根据协议，包长度字段在第7和8字节
      uint16_t packetLen = ((uint16_t)response[7] << 8) | response[8];
      // 包总长度 = 9(头+长度字段) + packetLen
      if (index >= 9 + packetLen) break;
    }
  }
}

// 之后处理 response，长度就是 index
printResponse(response, index);

  // 检查确认码
  if (index >= 28 && response[9] == 0x00) {
    uint16_t register_cnt = (uint16_t)(response[10]<<8) | response[11];
    uint16_t fp_temp_size = (uint16_t)(response[12]<<8) | response[13];
    uint16_t fp_lib_size  = (uint16_t)(response[14]<<8) | response[15];
    uint16_t score_level  = (uint16_t)(response[16]<<8) | response[17];
    uint32_t device_addr  = (uint32_t)(response[18]<<24) | (response[19]<<16)| (response[20]<<8) | response[21];
    uint16_t data_pack_size = (uint16_t)(response[22]<<8) | response[23];
    if(0 == data_pack_size) {
      data_pack_size = 32;
    }
    else if(1 == data_pack_size) {
      data_pack_size = 64;
    }
    else if(2 == data_pack_size) {
      data_pack_size = 128;
    }
    else if(3 == data_pack_size) {
      data_pack_size = 256;
    }
    uint16_t baud_set = (uint16_t)(response[24]<<8) | response[25];
    
    Serial.print("register cnt:");
    Serial.println(register_cnt);
    Serial.print("temp size:0x");
    Serial.println(fp_temp_size,HEX);
    Serial.print("lib size:");
    Serial.println(fp_lib_size);
    Serial.print("level:");
    Serial.println(score_level);
    Serial.print("devece address:0x");
    Serial.println(device_addr,HEX);
    Serial.print("data size:");
    Serial.println(data_pack_size);
    Serial.print("baud:");
    Serial.println(baud_set*9600);
    return 1;  // 成功
  } else {
      Serial.println("ERROR: Invalid response or command failed.");
    return 0; // 失败
  }
}



int register_FP(void)
{
  Serial.println("[DEBUG] register_FP start");
  BUFFER_ID = 1;
  while(BUFFER_ID <= 5) {
    Serial.print("[DEBUG] Getting image for buffer ");
    Serial.println(BUFFER_ID);
    send_cmd(CMD_GET_IMAGE);
    if (receiveResponse()) {
      Serial.println("[DEBUG] Get image success");
    } else {
      Serial.println("[DEBUG] Get image fail, retrying...");
      delay(1000);
      continue;
    }

    Serial.println("[DEBUG] Generating char...");
    send_cmd2(CMD_GEN_CHAR, BUFFER_ID);
    if (receiveResponse()) {
      Serial.println("[DEBUG] Gen char success");
      BUFFER_ID++;
    } else {
      Serial.println("[DEBUG] Gen char fail, retrying...");
      continue;
    }
  }

  Serial.println("[DEBUG] Merging model...");
  send_cmd(CMD_REG_MODEL);
  if (receiveResponse()) {
    Serial.println("[DEBUG] Merge model success");
  } else {
    Serial.println("[DEBUG] Merge model fail");
    return 0;
  }

  Serial.println("[DEBUG] Storing template...");
  BUFFER_ID = 1;
  sendCommand(CMD_STORE_CHAR, BUFFER_ID, TEMPLATE_ID);
  if (receiveResponse()) {
    Serial.println("[DEBUG] Store template success");
  } else {
    Serial.println("[DEBUG] Store template fail");
    return 0;
  }

  Serial.println("[DEBUG] register_FP success");
  return 1;
}


//搜索指纹
int search_FP(void)
{
  int serch_cnt = 0; 
  BUFFER_ID = 1;
  while(serch_cnt <= 5) {
    // 步骤1：获取图像
    send_cmd(CMD_GET_IMAGE);
  
    // 等待指纹模组响应
    if (receiveResponse()) {

    } else {
      delay(1000);
      continue;
    }
  
    // 步骤2：生成特征
    send_cmd2(CMD_GEN_CHAR, BUFFER_ID);
    if (receiveResponse()) {
      break;
    } else {
      serch_cnt++;
      delay(500);
      continue;
    }
  }

  // 步骤3：搜索指纹
  BUFFER_ID = 1;
  sendCommand1(CMD_SEARCH, BUFFER_ID, 1, 1);
  if (receiveResponse()) {
    return 1;
  }
  return 0;
}

//清空指纹库
int clear_FP_all_lib(void)
{
  send_cmd(CMD_CLEAR_LIB);
  if (receiveResponse()) {
    return 1;
  }
  return 0;
}

// 发送指令包
void send_cmd(uint8_t cmd) {
  uint8_t packet[12];
  uint16_t length=3;
  uint16_t checksum =  1+length+cmd;

  // 构建指令包
  packet[0] = HEADER_HIGH;
  packet[1] = HEADER_LOW;
  packet[2] = (DEVICE_ADDRESS >> 24) & 0xFF;
  packet[3] = (DEVICE_ADDRESS >> 16) & 0xFF;
  packet[4] = (DEVICE_ADDRESS >> 8) & 0xFF;
  packet[5] = DEVICE_ADDRESS & 0xFF;
  packet[6] = 0x01;  // 包标识：命令包
  packet[7] = (length >> 8) & 0xFF;  // 包长度高字节
  packet[8] = length & 0xFF;  // 包长度低字节
  packet[9] = cmd;
  
  packet[10] = (checksum >> 8) & 0xFF;
  packet[11] = checksum & 0xFF;

#if defined(HLK_DEBUG)
  Serial.println("send1:");
  printHex(packet,(2+4+3+length));
#endif

  // 发送指令包
  for (int i = 0; i < (2+4+3+length); i++) {
    fingerprintSerial.write(packet[i]);
  }
  
}
// 发送指令包
void send_cmd2(uint8_t cmd, uint8_t param1 ) {
  uint8_t packet[13];
  uint16_t length=4;
  uint16_t checksum =  1+length+cmd + param1;

  // 构建指令包
  packet[0] = HEADER_HIGH;
  packet[1] = HEADER_LOW;
  packet[2] = (DEVICE_ADDRESS >> 24) & 0xFF;
  packet[3] = (DEVICE_ADDRESS >> 16) & 0xFF;
  packet[4] = (DEVICE_ADDRESS >> 8) & 0xFF;
  packet[5] = DEVICE_ADDRESS & 0xFF;
  packet[6] = 0x01;  // 包标识：命令包
  packet[7] = (length >> 8) & 0xFF;  // 包长度高字节
  packet[8] = length & 0xFF;  // 包长度低字节
  packet[9] = cmd;
  packet[10] = param1;
  packet[11] = (checksum >> 8) & 0xFF;
  packet[12] = checksum & 0xFF;

#if defined(HLK_DEBUG)
  Serial.println("send2:");
  printHex(packet,(2+4+3+length));
#endif

  // 发送指令包
  for (int i = 0; i < (2+4+3+length); i++) {
    fingerprintSerial.write(packet[i]);
  }

}

// 发送指令包
void sendCommand(uint8_t cmd, uint8_t param1, uint16_t param2) {
  uint8_t packet[15];
  uint16_t length=6;
  uint16_t checksum =  1+length+cmd + param1 + (param2 >> 8) + (param2 & 0xFF);

  // 构建指令包
  packet[0] = HEADER_HIGH;
  packet[1] = HEADER_LOW;
  packet[2] = (DEVICE_ADDRESS >> 24) & 0xFF;
  packet[3] = (DEVICE_ADDRESS >> 16) & 0xFF;
  packet[4] = (DEVICE_ADDRESS >> 8) & 0xFF;
  packet[5] = DEVICE_ADDRESS & 0xFF;
  packet[6] = 0x01;  // 包标识：命令包
  packet[7] = (length >> 8) & 0xFF;  // 包长度高字节
  packet[8] = length & 0xFF;  // 包长度低字节
  packet[9] = cmd;
  packet[10] = param1;
  packet[11] = (param2 >> 8) & 0xFF;
  packet[12] = param2 & 0xFF;
  packet[13] = (checksum >> 8) & 0xFF;
  packet[14] = checksum & 0xFF;

#if defined(HLK_DEBUG)
  Serial.println("send:");
  printHex(packet,(2+4+3+length));
#endif

  // 发送指令包
  for (int i = 0; i < (2+4+3+length); i++) {
    fingerprintSerial.write(packet[i]);
  }
}

// 发送指令包
void sendCommand1(uint8_t cmd, uint8_t param1, uint16_t param2, uint16_t param3) {
  uint8_t packet[17];
  uint16_t length=8;
  uint16_t checksum =  1+length+cmd + param1 + (param2 >> 8) + (param2 & 0xFF) + (param3 >> 8) + (param3 & 0xFF);

  // 构建指令包
  packet[0] = HEADER_HIGH;
  packet[1] = HEADER_LOW;
  packet[2] = (DEVICE_ADDRESS >> 24) & 0xFF;
  packet[3] = (DEVICE_ADDRESS >> 16) & 0xFF;
  packet[4] = (DEVICE_ADDRESS >> 8) & 0xFF;
  packet[5] = DEVICE_ADDRESS & 0xFF;
  packet[6] = 0x01;  // 包标识：命令包
  packet[7] = (length >> 8) & 0xFF;  // 包长度高字节
  packet[8] = length & 0xFF;  // 包长度低字节
  packet[9] = cmd;
  packet[10] = param1;
  packet[11] = (param2 >> 8) & 0xFF;
  packet[12] = param2 & 0xFF;
  packet[13] = (param3 >> 8) & 0xFF;
  packet[14] = param3 & 0xFF;
  packet[15] = (checksum >> 8) & 0xFF;
  packet[16] = checksum & 0xFF;

#if defined(HLK_DEBUG)
  Serial.println("send:");
  printHex(packet,(2+4+3+length));
#endif

  // 发送指令包
  for (int i = 0; i < (2+4+3+length); i++) {
    fingerprintSerial.write(packet[i]);
  }
}



// 打印响应包
void printResponse(uint8_t *response, uint8_t length) {
  Serial.print("Response:");
  for (int i = 0; i < length; i++) {
    if(response[i] < 0x10) Serial.print('0');
    Serial.print(response[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}
bool receiveResponse() {
  uint8_t response[64];
  uint8_t index = 0;
  uint32_t startTime = millis();

  // 先等待包头 EF 01
  bool headerFound = false;

  while (millis() - startTime < 2000) {  // 延长等待时间到2秒
    if (fingerprintSerial.available()) {
      uint8_t b = fingerprintSerial.read();

      if (!headerFound) {
        if (index == 0 && b == 0xEF) {
          response[index++] = b;
        } else if (index == 1 && b == 0x01) {
          response[index++] = b;
          headerFound = true;
        } else {
          index = 0; // 重新开始找包头
        }
      } else {
        response[index++] = b;

        if (index >= 9) {
          // 包长度在第7和8字节
          uint16_t packetLen = ((uint16_t)response[7] << 8) | response[8];
          if (index >= 9 + packetLen) {
            // 收到完整包
            break;
          }
        }
      }
    }
  }

#if defined(HLK_DEBUG)
  Serial.println("Received response:");
  printResponse(response, index);
#endif

  // 简单校验包头和确认码
  if (index >= 12 && response[0] == 0xEF && response[1] == 0x01 && response[9] == 0x00) {
    return true;
  }
  return false;
}
void updatefingerprint() {
  if(!isInit) { //上电打印模组基本参数
    isInit = true;
    read_FP_info();

    //命令使用说明
    command_use();
  }

  //接收指令
  run_command();

 // delay(100);
}